'''
Concatenation and decimation of GPS binary files
Suported formats:
 - Novatel
 - Leica
 - Ashtech

@author: Juan C. Espinoza
@contact: jucar.espinoza@gmail.com
October 30, 2009

CHANGELOG:

version 1.5.1
- cat_files now search files recursively in source

version 1.5
- move rinex conversion functions to Rinex module

version 1.4.2, 14/03/11
- save scintillation leica files into scinda format.

version 1.4.1, 14/03/10
- add download method to Yuma class.
- fix fdate bad attribute in Yuma class (replace for date).

version 1.4, 14/03/10
- add Yuma class to read almanacs files in YUMA format
- add gpselaz class (calculate el, az, lat and lon from yuma data)
- create mkRinex class with rinex conversion functions.

version 1.3, 14/01/10
- add obs support (cat_files, obs_to_rnx)
- unify functions cat_nvd, cat_lb2 in cat_files
- unify class decimate_nvd, decimate_lb2 in DecimateFile
- create new functions for rinex conversion (rnx_sat, rnx_record).
- rinex converssion support more than 12 sat in records;

version 1.2, 06/01/10
- add lb2 support (cat_lb2, lb2_to_rnx)
- fix errors when running in a 64-bit plataform

version 1
- Initial release

'''
import os, re, sys, gzip, math, traceback
from warnings import warn
from struct import pack, unpack
from urllib2 import urlopen
from datetime import datetime, timedelta
from Utility import open_file, fmod, PI, deg2rad, rad2deg, rglob, set
from gpsdatetime import GPSDateTime
from headers import *

try:
    from liblisn import CRC32
except ImportError:
    warn('Error importing extension liblisn NVD conversion will be not available')

__version__ = '1.5.1'

#globals
jday_gps0   = 2444245
w_l1        = 0.190293672983469
w_l2        = 0.2442102134245683

kdays     = [31,28,31,30,31,30,31,31,30,31,30,31]

localpath = './'

def limit(x):
    '''
    Limit the value of x to 10^10 (RINEX limitation)
    '''
    if x<0: return -(abs(x)%1000000000)
    return x%1000000000

def gdate(jd):
    '''
    Computes the Gregorian Calendar date (year, month, day)
    given the Julian date (jd)
    Translate to python from write_daily_nvd_file.c of Robert Sheehan
    <sheehanp@bc.edu>.
    '''
    l= jd+68569
    n= 4*l/146097
    l= l-(146097*n+3)/4
    i= 4000*(l+1)/1461001
    l= l-1461*i/4+31
    j= 80*l/2447
    k= l-2447*j/80
    l= j/11
    j= j+2-12*l
    i= 100*(n-49)+i+l

    return (int(i), int(j), int(k))

def jd(yy, mm, dd):
    '''
    Computes the Julian date (jd) given a Gregorian Calendar date
    (year, month, day).
    Translate to python from write_daily_nvd_file.c of Robert Sheehan
    <sheehanp@bc.edu>.
    '''
    return dd-32075+1461*(yy+4800+(mm-14)/12)/4+ \
                367*(mm-2-(mm-14)/12*12)/12-3*((yy+4900+(mm-14)/12)/100)/4

def min2date(mins):
    '''
    Converts time in minutes from january 1th into day of year, hour and minutes
    '''
    mm  = mins%60
    dif = (mins - mm)/60
    hh = dif%24
    doy = (dif - hh)/24+1
    return doy, hh, mm

def lla2xyz(lat, lon, alt):
    '''
    Convert lat, lon, height in WGS84 to ECEF (X,Y,Z)
    lat and long given in decimal degrees,altitude should be given in meters
    '''
    lat = lat*deg2rad
    lon = lon*deg2rad
    a   = 6378137.0
    f   = 1/298.257223563
    e2  = 2*f-f**2
    chi = math.sqrt(1-e2*(math.sin(lat))**2)
    X   = (a/chi +alt)*math.cos(lat)*math.cos(lon)
    Y   = (a/chi +alt)*math.cos(lat)*math.sin(lon)
    Z   = (a*(1-e2)/chi + alt)*math.sin(lat)
    return X,Y,Z

def xyz2lla(x, y, z):
    '''
    Calculate geodetic location from x,y and z
    lat & lon in degrees and alt in same units as .
    '''
    a   = 6378137.0
    e   = 8.1819190842622e-2
    b   = math.sqrt(a**2*(1-e**2))
    ep  = math.sqrt((a**2-b**2)/b**2)
    p   = math.sqrt(x**2+y**2)
    th  = math.atan2(a*z, b*p)
    lon = math.atan2(y, x)
    lat = math.atan2((z+ep**2*b*math.sin(th)**3), (p-e**2*a*math.cos(th)**3))
    N   = a/math.sqrt(1-e**2*math.sin(lat)**2)
    alt = p/math.cos(lat)-N
    
    return lat*rad2deg, lon*rad2deg, alt

    '''
    r = sqrt(x**2+y**2+z**2)
    if r < 6370000:
        r = 6370000
    xlat = atan(z/(sqrt(x**2+y**2)))
    xlat = xlat*rad2deg
    xlon = atan(y/x)
    if x >= 0.:
        xlon = xlon*rad2deg
    if x < 0.:
        if y >= 0.:
            xlon = (xlon*rad2deg) + 180.
        if y < 0.:
            xlon = (xlon*rad2deg) - 180.
    RE = 6378390*(0.99832 + 0.00168*cos(2.0*xlat/rad2deg))
    r -= RE
    return xlat, xlon, r
    '''

def TRNN(glat, glon, R):
    '''
    GEOCENTRIC TO GEODETIC CONVERSION
    GEOCENTRIC glat (RAD), glon (RAD), RADIUS (KM)
    SGD IS GEODETIC LAT (RAD), LONG (RAD), RADIUS (KM)
    SGR IS GEODETIC LAT (DEG), LONG (DEG), ALTITUDE (KM)
    translate to python from gpselem.f
    '''

    XC = [0,0,0]
    SGD = [0,0,0]
    SGR = [0,0,0]
    A = 6378.135
    B = 6356.75
    F     = (A - B) / A
    EP2   = 2.0 * F - F * F
    EP4   = EP2 * EP2
    C8    = 1.0 - EP2
    C5    = 2.0 * A**2 * EP2 * C8
    C6    = A**2 * C8 * C8
    C1    = -EP4 / C8
    C2    = -2.0 * EP2
    C3    = A**2 * EP4
    RO    = R * math.sin(glat)
    XC[0] = RO * math.cos(glon)
    XC[1] = RO * math.sin(glon)
    XC[2] = R * math.cos(glat)
    RO2   = RO * RO
    Z2    = XC[2] * XC[2]
    Z0    = A * XC[2] * math.sqrt(C8/(C8*RO2+Z2))
    E1    = 4.0 * C1
    D2    = C2 * XC[2]
    E2    = 3.0 * D2
    D3    = C3 - C8 * Z2 - RO2
    E3    = 2.0 * D3
    D4    = C5 * XC[2]
    D5    = C6 * Z2
    TEST = 1
    while (TEST-0.00001>0):
        FZ0   = (((C1*Z0+D2)*Z0+D3)*Z0+D4)*Z0+D5
        FPZ   = ((E1*Z0+E2)*Z0+E3)*Z0+D4
        Z1 = Z0 - FZ0/FPZ
        TEST=abs(Z1-Z0)
        Z0 = Z1
    SGD[1]=glon
    SGD[0]=math.atan(math.sqrt(C8*(A**2*C8-Z1*Z1))/Z1)
    if(SGD[0]<0):
        SGD[0]=SGD[0]+PI
    SPLT=math.cos(SGD[0])
    SGD[2]=XC[2]/SPLT-A*C8/math.sqrt(1.-EP2*SPLT*SPLT)
    GLT=PI/2-SGD[0]
    ALR=A*(.998320047+.001683494*math.cos(2.*GLT)-.000003549*math.cos(4.*GLT)+\
            0.000000008*math.cos(6.*GLT))
    SGR[0]=GLT*rad2deg
    SGR[1]=SGD[1]*rad2deg
    SGR[2]=SGD[2]
    SGD[2]=SGD[2]+ALR
    return SGD,SGR

def SILL(EL,AZ,XH,XTLON,XTLAT):
    '''
    THIS ROUTINE COMPUTES SUB-IONOSPHERIC LATITUDE AND
    LONGITUDE, FROM STATION COORDINATES, AND IONOSPHERIC
    HEIGHT.
    translate to python from gpselem.f
    '''

    ELR   = deg2rad*EL
    AZR   = deg2rad*AZ
    STLAT = deg2rad*XTLAT
    STLON = deg2rad*XTLON
    RE    = 6378.39*(0.99832 + 0.00168*math.cos(2.0*STLAT ))
    CHI   = math.asin(math.cos(ELR)*RE/(RE + XH))
    A     = 1.5707963 - CHI - ELR
    #SILAT=ASIN(SIN(STLAT)*COS(A)+COS(STLAT)*SIN(A)*COS(AZ))
    ARG = math.sin(STLAT)*math.cos(A)+math.cos(STLAT)*math.sin(A)*math.cos(AZR)
    if (abs(ARG) > 1.0):
        print 'ARCSIN ARGUMENT ERROR IN SILL FOR SILAT'
        print 'ARG = ', ARG
        print 'ELR = ', ELR, '  AZR = ', AZR
        if ARG<0:
            ARG = -1.0
        elif ARG>0:
            ARG = 1.0
        else:
            ARG = 0.0
    SILAT = math.asin(ARG)
    #DLONG=ASIN(SIN(A)*SIN(AZ)/COS(SILAT))
    ARG = math.sin(A)*math.sin(AZR)/math.cos(SILAT)
    if (abs(ARG) > 1.0):
        print 'ARCSIN ARGUMENT ERROR IN SILL FOR DLONG'
        print 'ARG = ', ARG
        print 'ELR = ', ELR, '  AZR = ', AZR
        if ARG<0:
            ARG = -1.0
        elif ARG>0:
            ARG = 1.0
        else:
            ARG = 0.0
    DLONG = math.asin(ARG)
    SILON = STLON+DLONG
    RANGE = (RE+XH)*math.sin(A)/math.cos(ELR)
    '''
    SPECIAL...CALCULATION OF SUB-IONOSPHERIC COORDINATES WAS DONE IN
    GEODETIC SYSTEM.  THEN SUB-IONO. LAT IS CHANGED INTO GEOCENTRIC
    FOR SUB-ROUTINE GEO----     JAK MOD. FEB 21,1973...
    SILAT STAYS IN GEOCENTRIC TO GO INTO SUBROUTINE GEO........
    '''
    SILAT = rad2deg*SILAT
    SILON = rad2deg*SILON
    A = rad2deg*A
    return SILAT,SILON,A,RANGE

def cat_files(file_out, source, run_date=None, interval=10, extension='',
              old_extension='', label='', header='', gz=False, daily=True):
    '''
    Concatenate files from "source" (also can deal with gziped files) and create
    a new decimated file depending of extension.
    
    Inputs:
        file_out  : file name for the new file
        source    : path where the files are located can be also a list of files
        run_date  : date to filter source files [datetime]
        interval  : interval in seconds
        extension : type of file: nvd, lb2, obs, txt
        old_extension : extension of the files in source
        label     : label used to filter files
        header    : TODO
        gz        : Gzip the new file
        daily     : Used for scintillation files        
    Output:
        Boolean   : True if the new file has valid records

    '''
    if isinstance(source, list):
        all_files = [f for f in source]
        file_list = [f for f in source]
    elif isinstance(source, str):
        if isinstance(run_date, datetime):
            run_date0 = run_date-timedelta(1)
            run_date1 = run_date+timedelta(1)
            filter  = '*%s*%s*.%s' % (label, run_date.strftime('%y%m%d'),
                                      old_extension)
            filter0 = '*%s*%s*.%s' % (label, run_date0.strftime('%y%m%d'),
                                      old_extension)
            filter1 = '*%s*%s*.%s' % (label, run_date1.strftime('%y%m%d'),
                                      old_extension)
            file_list0 = rglob(source, filter0)
            file_list1 = rglob(source, filter1)
            file_list0.sort()
            file_list1.sort()
        else:
            filter = '*.%s' % ext
        file_list = rglob(source, filter)
        file_list.sort()
        if isinstance(run_date, datetime):
            all_files = file_list0[-20:]+file_list+file_list1[:5]
        else:
            all_files = [f for f in file_list]
        if len(file_list) == 0:
            print 'No files found at:', source
            return False
    else:
        print 'Invalid source'
        return False

    print 'Creating %s from *%s*.%s files' % (file_out, label, old_extension)
    dec_file     = DecimateFile(file_out, extension, 
                                '%s.%s' % (label, old_extension), run_date)
    last_gps_sec = 0
    last_sec     = -1
    line         = ''
    for file in all_files:
        print ' Processing: %s' % file
        try:
            fileo = open_file(file)
        except IOError:
            print ' Error reading file:', file, 'skipping'
            continue
        if extension=='nvd':
            last_gps_sec = dec_file.add_nvd(fileo, last_gps_sec, interval)
        elif extension=='lb2':
            last_gps_sec = dec_file.add_lb2(fileo, last_gps_sec, interval)
        elif extension=='obs':
            last_gps_sec = dec_file.add_obs(fileo, last_gps_sec, interval)
        else:
            if 'txt' in old_extension:
                line = open(file_list[0]).readline()
            last_sec = dec_file.add_file(fileo, last_sec, interval, line, daily)
        fileo.close()
    dec_file.close(gz)
    if dec_file.nrec<>0:
        return True
    return False

class ReadRecord(dict):
    '''
    Class to unpack binary string or buffer into data values.
    '''
    def __init__(self, format=None, buffer=None):
        if format is not None and buffer is not None:
            self.vals = unpack(format, buffer)
        elif format is None and buffer is not None:
            self.vals = [x.strip() for x in buffer.split(' ') if len(x)<>0]
        self.prn = 0

    def id43_header(self):
        '''
        unpack id43 header (nvd file)
        '''
        self.msg_id        = self.vals[0]    #unsigned short
        self.msg_type      = self.vals[1]    #char
        self.port_add      = self.vals[2]    #char
        self.msg_len       = self.vals[3]    #unsigned short
        self.sequence      = self.vals[4]    #unsigned short
        self.idle_time     = self.vals[5]    #char
        self.time_status   = self.vals[6]    #char
        self.week          = self.vals[7]    #unsigned short
        self.milli_sec     = self.vals[8]    #long
        self.receiver_stat = self.vals[9]    #unsigned long
        self.reserved      = self.vals[10]   #unsigned short
        self.sw_version    = self.vals[11]   #unsigned short

    def id43(self):
        '''
        unpack id43 record of nvd file
        '''

        class track_status(object):
            '''
            Class to hold track status of a nvd record
            '''
            def __init__(self, track_stat):
                self.phase_lock = (track_stat >> 10) & 0x01
                self.parity     = (track_stat >> 11) & 0x01
                self.code_lock  = (track_stat >> 12) & 0x01
                self.group      = (track_stat >> 20) & 0x01
                self.freq       = (track_stat >> 21) & 0x03
                self.code       = (track_stat >> 23) & 0x07

        track = track_status(self.vals[9])

        self.prn        = self.vals[0]    #unsigned short
        self.reserved   = self.vals[1]    #unsigned short
        if track.freq == 0:
            self['C1']  = self.vals[2]    #double
            self['L1']  = limit(self.vals[4]*-1) #double
            self['S1']  = self.vals[7]    #float
        if track.freq == 1:
            self['P2']  = self.vals[2]    #double
            self['L2']  = limit(self.vals[4]*-1) #double
            self['S2']  = self.vals[7]    #float
        self.psr_std    = self.vals[3]    #float
        self.adr_std    = self.vals[5]    #float
        self.dopp       = self.vals[6]    #float
        self.locktime   = self.vals[8]    #float

    def id3_header(self):
        '''
        '''
        temp = self.vals[0:3]+('\x00',)
        self.gpstow        = unpack('=I', ''.join(temp))[0]
        self.accum         = self.vals[3]
        self.iflag         = self.vals[4]

    def id3(self, buffer, ind):
        '''
        '''
        self.dopp = self.vals[0]
        L1denb = (self.dopp >> 3) & 0x01
        L2denb = (self.dopp >> 4) & 0x01
        stat = self.vals[1]
        chan = (stat & 0x07);
        L1trk = ((stat >> 4) & 0x03)
        L2trk = ((stat >> 6) & 0x03)
        L1lck = ((stat >> 9) & 0x01)
        L2lck = ((stat >> 10) & 0x01)
        self.prn = ((stat >> 11) & 0x1f)+1
        self.L1qual = 0
        self['L1'] = 0.
        self.L1code = 0
        self.L1contrk = 0
        self.L1dopp = 0
        self.L1costi = 0
        self.L1cn0 = 0
        if L1trk == True:
            data = unpack('=BdhB', buffer[ind:ind+12])
            ind += 12
            self.L1qual = data[0]
            self['L1'] = limit(data[1])
            self.L1code = limit(data[2])
            self.L1contrk = data[3]
            if L1denb == True:
                data = unpack('=L', buffer[ind:ind+4])
                ind += 4
                self.L1dopp = data[0]
            self.L1costi = self.L1qual & 0x7
            self.L1cn0 = self.L1qual >> 3
        self.L2qual = 0
        self['L2'] = 0.
        self.L2code = 0
        self.L2contrk = 0
        self.L2dopp = 0
        if L2trk == True:
            data = unpack('=BdhB', buffer[ind:ind+12])
            ind += 12
            self.L2qual = data[0]
            self['L2'] = limit(data[1])
            self.L2code = limit(data[2])
            self.L2contrk = data[3]
            if L2denb == True:
                data = unpack('=L', buffer[ind:ind+4])
                ind += 4
                self.L2dopp = data[0]
            self.L2costi = self.L2qual & 0x7
            self.L2cn0 = self.L2qual >> 3
        self['C1'] = (float(self['L1']) + self.L1code/32.) * w_l1
        self['P2'] = (float(self['L2']) + self.L2code/32.) * w_l2
        self.ind = ind

    def id4(self):
        self.prn = self.vals[0]
        self['az'] = self.vals[1]
        self['el'] = self.vals[2]

    def id37_header(self):
        self.pc_date       = self.vals[0:3]    #char
        self.pc_time       = self.vals[3:6]    #char
        self.dhms          = self.vals[6:10]   #char
        self.nprn          = self.vals[10]     #char
        if self.pc_date[0] < 90:
            juld_pc = jd(self.pc_date[0]+2000, self.pc_date[1], self.pc_date[2])
        else:
            juld_pc = jd(self.pc_date[0]+1900, self.pc_date[1], self.pc_date[2])
        idow_pc = (juld_pc % 7) + 1
        idow_gps = self.dhms[0]
        del_dow = idow_pc - idow_gps
        if del_dow == 0:
            juld_gps = juld_pc
        else:
            if del_dow > 1:
                del_dow = del_dow - 7
            if del_dow < -1:
                del_dow = del_dow + 7
            juld_gps = juld_pc - del_dow
        idsec = 3600*self.dhms[1] + 60*self.dhms[2] + self.dhms[3]
        idsec_pc = 3600*self.pc_time[0] + 60*self.pc_time[1] + self.pc_time[2]
        gps_day = juld_gps - jday_gps0
        self.gps_sec = gps_day * 86400 + idsec
        self.gps_date = gdate(juld_gps)

    def id37(self):
        '''
        unpack id37 record of lb2 file
        '''
        self.prn     = self.vals[0] + 1   #unsigned short
        self['L1']   = limit(self.vals[1])       #double
        self.code1   = self.vals[2]       #short
        self['L2']   = limit(self.vals[3])       #double
        self.code2   = self.vals[4]       #short
        #self.el      = self.vals[4]      #unsigned short
        #self.az      = self.vals[5]      #unsigned short
        self['C1']   = (self['L1'] + self.code1/32.0) * w_l1
        self['P2']   = (self['L2'] + self.code2/32.0) * w_l2

    def id85(self, buffer):
        '''
        unpack id85 record of lb2 file
        '''
        ind = 1
        iflag = self.vals[0]
        if iflag and 0x1:
            self.date = unpack('3B', buffer[1:4])
            self.time = unpack('3B', buffer[4:7])
            self.offset = unpack('h', buffer[7:9])
            ind+=8
        if (iflag>>1) and 0x1:
            ind+=1
            llh = unpack('=3d', buffer[ind:ind+24])
            self.lat = llh[0]
            self.lon = llh[1]
            self.hgt = llh[2]
            ind += 24
        self.ind = ind

    def obs_line(self, pcode):
        '''
        unpack obs line record of obs file
        '''
        self.millisec = int(self.vals[0])
        #self.warning_CA = int(self.vals[1])
        #self.warning_P1 = int(self.vals[2])
        #self.warning_P2 = int(self.vals[3])
        self['S1'] = float(self.vals[4])-30
        self['S2'] = float(self.vals[5])-30
        self[pcode] = float(self.vals[6])
        self['P2'] = float(self.vals[7]) + float(self.vals[6])
        self['L1'] = limit(float(self.vals[8]))
        self['L2'] = limit(float(self.vals[9]))+ limit(float(self.vals[8]))*60/77
        self.prn = int(self.vals[10])

class DecimateFile(object):
    '''
    Class to create a new file with decimated values.
    '''

    def __init__(self, file_name=None, extension='dat', old_extension='', date=None):
        if file_name is None:
            file_name = os.path.join(localpath, 'abcd.'+extension)
        self.file_name  = file_name
        self.old_extension   = old_extension
        self.fileo      = os.tmpfile()
        self.data       = ''
        self.nrec       = 0
        self.first_line = None
        self.fix_line   = ''
        self.date       = date
        self.bad_doy    = None
        self.doy        = 0
        self.last_prns  = []

    def add_nvd(self, filei, gps_sec_last=0, interval=10):
        '''
        Add new raw nvd data files to the decimated new file.
        Based in write_daily_nvd_file.c of Robert Sheehan <sheehanp@bc.edu>

        Inputs:
            filei: input file object
            gps_sec_last: seconds (gps time) to start append new data
            interval: decimate value in seconds

        Outputs:
            gps_sec_last: seconds (gps time) for the last data block appened.
        '''
        nrec = 0
        sync = [0, 0, 0]
        while True:
            try:
                ch = unpack('B', filei.read(1))[0]
            except:
                break
            sync = [sync[1], sync[2], ch]
            #print old_pos, sync
            if sync == [0xAA, 0x44, 0x12]:
                file_pos = filei.tell()
                buff0 = filei.read(1)
                hdr_len = unpack('B', buff0)[0]
                buff1 = filei.read(hdr_len-4)
                try:
                    header = ReadRecord('=HBBHHBBHlLHH', buff1)
                except:
                    filei.seek(file_pos)
                    continue
                header.id43_header()
                week_sec = header.milli_sec/1000
                gps_sec = header.week*SECONDS_IN_WEEK + week_sec
                dow = week_sec/86400
                daysec = week_sec%86400
                hh = daysec/3600
                mm = (daysec%3600)/60
                ss = daysec%60
                rec_date = gdate(header.week*7 + dow + jday_gps0) + (hh, mm, ss)
                fdate = datetime(*rec_date[:3])

                if self.date:
                    if fdate < self.date:
                        #4 for crc 3 for sync
                        filei.seek(file_pos+hdr_len+header.msg_len+(4-3))
                        continue
                    elif fdate > self.date:
                        print 'Truncate file:', filei.name
                        break

                if header.msg_len < 8000:
                    buff2 = filei.read(header.msg_len)
                    buff = '\xAA\x44\x12'+buff0+buff1+buff2
                    try: crc_1 = unpack('=L', filei.read(4))[0]
                    except: continue
                    crc_2 = CRC32(buff) & 0xFFFFFFFF
                    if crc_1 <> crc_2:
                        filei.seek(file_pos, 0)
                        print 'CRC_error:', crc_1, crc_2, rec_date
                        continue
                    else:
                        is_dec = gps_sec%interval == 0
                        if ((header.msg_id == 43) and is_dec):
                            if gps_sec > gps_sec_last:
                                gps_sec_last = gps_sec
                                self.fileo.write(buff)
                                self.fileo.write(pack('=L', crc_1))
                                nrec += 1
                else:
                    filei.seek(file_pos, 0)
        self.nrec += nrec
        return gps_sec_last

    def add_lb2(self, fileo, gps_sec_last=0, interval=10):
        '''
        Add new binary id37 data files to the decimated new file,
        based in write_daily_id37_file.c of Robert Sheehan <sheehanp@bc.edu>.

        Inputs:
            fileo: binary id37 object file
            gps_sec_last: seconds (gps time) to start append new data
            interval: id37 sample interval in seconds
            run_date: datetime to restrict data for the given date.

        Outputs:
            gps_sec_last: seconds (gps time) for the last data block appened.
        '''

        nrec = 0
        sync = [0, 0]
        while True:
            try:
                ch = unpack('B', fileo.read(1))[0]
            except:
                break
            sync = [sync[1], ch]
            #print old_pos, sync
            if sync == [0x9C, 0xAE]:
                file_pos = fileo.tell()
                buff0 = fileo.read(2)
                if len(buff0)<2: break
                rec_len = unpack('H', buff0)[0]
                if rec_len >8192:
                    fileo.seek(file_pos, 0)
                    continue
                buff1 = fileo.read(1)
                if len(buff1)<1: break
                rec_id = unpack('B', buff1)[0]
                buff2 = fileo.read(rec_len-5)
                if len(buff2)<(rec_len-5): break
                buff3 = fileo.read(2)
                if len(buff3)<2: break
                checksum1 = unpack('h', buff3)[0]
                num_bytes = 3+rec_len-5
                checksum2 = -1*sum(unpack('%iB' % num_bytes, buff0+buff1+buff2))
                #print checksum1, checksum2
                if checksum1 <> checksum2:
                    print 'Checsum error'
                    fileo.seek(file_pos, 0)
                    continue
                if rec_id == 0x37:
                    station_prefix = buff2[:2]
                    i0 = 2
                    if buff2[3:6] == 'ver':
                        print 'ver True'
                        i0 += 10
                    #format = '%iB' % len(buff2[i0:])
                    format = '11B'
                    header = ReadRecord(format, buff2[i0:i0+11])
                    header.id37_header()
                    fdate = datetime(*header.gps_date)
                    if self.date:
                        if fdate < self.date:
                            fileo.seek(file_pos+rec_len, 0)
                            continue
                        elif fdate > self.date:
                            print 'Truncate file:', fileo.name
                            break
                    is_dec = header.gps_sec%interval == 0
                    if is_dec and (header.gps_sec>gps_sec_last):
                        gps_sec_last = header.gps_sec
                        self.fileo.write('\x9C\xAE'+buff0+buff1+buff2)
                        self.fileo.write(pack('h', checksum1))
                        nrec += 1
        self.nrec += nrec
        return gps_sec_last

    def add_obs(self, filei, gps_sec_last=0, interval=10):
        '''
        Add new obs data files to the decimated new file.

        Inputs:
            filei: input file object
            gps_sec_last: seconds (gps time) to start append new data
            interval: decimate value in seconds

        Outputs:
            gps_sec_last: seconds (gps time) for the last data block appened.
        '''

        nrec = 0
        if not self.date:
            try:
                week = GPSDateTime(filei.name.split('/')[-1]).GPSweek
            except:
                print '%s does not have a valid datestamp yymmdd, skipping...' % filei.name
                return gps_sec_last
        else:
            week = self.date.GPSweek
            gps_sec_min = week*SECONDS_IN_WEEK+int(self.date.strftime('%w'))*86400
            gps_sec_max = gps_sec_min+86400

        while True:
            buff = filei.readline()
            if not buff:
                break
            values = [x.strip() for x in buff.split(' ') if len(x)<>0]
            if len(values)<>11:
                continue
            try:
                week_sec = int(values[0])/1000
                dum = int(values[1])
                dum = int(values[2])
                dum = int(values[3])
            except:
                continue
            gps_sec = week*SECONDS_IN_WEEK + week_sec

            if self.date and gps_sec>=gps_sec_max:
                continue
            if self.date and gps_sec<gps_sec_min:
                continue
            '''
            dow = week_sec/86400
            daysec = week_sec%86400
            hh = daysec/3600
            mm = (daysec%3600)/60
            ss = daysec%60
            rec_date = gdate(week*7 + dow + jday_gps0) + (hh, mm, ss)
            fdate = datetime(*rec_date[:3])
            #print fdate
            '''
            if gps_sec>gps_sec_last:
                self.last_prns = []
            is_dec = gps_sec%interval == 0
            if gps_sec >= gps_sec_last and is_dec:
                gps_sec_last = gps_sec
                if values[-1] not in self.last_prns:
                    self.last_prns.append(values[-1])
                    self.fileo.write(buff)
                    nrec += 1
        self.nrec += nrec
        return gps_sec_last

    def add_file(self, filei, last_sec, interval, line, daily=True):
        '''
        Add new text data files to the decimated new file.

        Inputs:
            filei: input file object
            gps_sec_last: seconds (gps time) to start append new data
            interval: decimate value in seconds

        Outputs:
            gps_sec_last: seconds (gps time) for the last data block appened.
        '''

        if self.first_line is None:
            if not self.date:
                self.date = GPSDateTime(filei.name.split('/')[-1])
            if 'txt' in self.old_extension:
                self.doy        = self.date.doy
                self.first_line = line
                self.fix_line   = line[3:15].split(' ')
                if not self.bad_doy:
                    self.bad_doy = int([s.strip() for s in line.split(' ') if s][3])
                self.fix_line = '%s%s%02d' % (self.fix_line[0], self.fix_line[1],
                                int(self.fix_line[2]))
            elif 's4' in self.old_extension or 'pos' in self.old_extension:
                self.first_line = '%4d %02d %02d\n' % (self.date.year,
                                    self.date.month, self.date.day)
                self.fix_line = '%02d %003d ' % (self.date.year%100,
                                                 self.date.doy)
            elif 'scn' in self.old_extension:
                self.first_line = 'T'
                self.fix_line = 'T %02d %02d %02d' % (self.date.year%100,
                                    self.date.month, self.date.day)

        if self.fix_line == '000000' and (self.date.strftime('%y%m%d') \
                not in filei.name):
            print 'skipping,', filei.name
            return last_sec

        nrec = 0
        if 'txt' in self.old_extension:
            buff = filei.readline()
        while True:
            buff = filei.readline()
            if not buff:
                break
            #if re.findall('[^\x0a\x20-\x7f]', buff):
            #    print 'Error chars'
            #    continue
            try:
                values = [x.strip() for x in buff.split(' ') if x]
                if 's4.gz' in self.old_extension:
                    sec = int(values[1]+values[2])
                elif 'txt' in self.old_extension:
                    str_sec = values[1]
                    sec = int(values[0][-2:])*100000 + (int(str_sec[:2])*60*60 + int(str_sec[2:4])*60 + int(str_sec[4:6]))
                    if int(values[0][-2:])>self.bad_doy:
                        self.bad_doy += 1
                        self.doy += 1
                        #sec += 100000
                    nsats = int(values[2])
                elif 'pos' in self.old_extension:
                    sec = int(values[2]+values[3])
                elif 'scn' in self.old_extension:
                    if values[0]<>'T':
                        continue
                    sec = int(''.join(values[1:]))
                else:
                    self.fileo.write(buff)
            except:
                traceback.print_exc(4)
                continue
            if self.fix_line in buff and self.nrec<>0 and sec==0:
                print 'first obs found'
                self.data  = ''
                last_sec   = -1
            if (self.fix_line in buff or not daily) and sec>last_sec:
                if 'txt' in self.old_extension:
                    #if nrec>1 and sec-last_sec>3000: continue
                    fmt0 = '%02d %03d% 6d% 3d'
                    fmt1 = ['% 3d %3.2f %4.1f %3.1f' for i in range(nsats)]
                    fmt  = fmt0+''.join(fmt1)+'\n'
                    str_sec = values[1]
                    vals = (GPSDateTime(filei.name.split('/')[-1]).year%100, self.doy, sec-int(values[0][-2:])*100000, nsats)
                    vals += tuple([float(s.replace(',', '.')) for s in values[3:] if s])
                    self.data += fmt % vals
                elif 'scn' in self.old_extension:
                    self.data += buff
                    while True:
                        nline = filei.readline()
                        if 'T' in nline:
                            filei.backline()
                            break
                        elif nline:
                            self.data += nline
                        else:
                            break
                else:
                    self.data += buff.strip()+'\n'
                last_sec = sec
                nrec += 1
        self.nrec += nrec
        return last_sec

    def close(self, gz=False, header=None):
        '''
        Save and close files, create path if not exist
        '''
        self.fileo.seek(0)
        if self.nrec <> 0:
            path = os.path.dirname(self.file_name)
            if not os.path.exists(path):
                if path<>'':
                    os.makedirs(os.path.dirname(self.file_name))
            if gz:
                print 'Gziping file'
                new_file = gzip.open(self.file_name+'.gz', 'wb')
            else:
                new_file = open(self.file_name, 'wb')
            if self.data:
                new_file.write(self.data)
            else:
                new_file.write(self.fileo.read())
            new_file.close()
        self.fileo.close()
        print 'Saved %d records' % self.nrec

class Yuma(dict):
    '''
    Class to read an hold data from almanacs files (download if neccesary)
    in yuma format.
    Search almanac file for the given date (datetime) and path.
    '''
    def __init__(self, date, path=None, N=31, local=False):
        '''
        '''
        self.date = date
        if not path:
            path = os.path.join(localpath, 'almanac')
        self.path = path
        if not os.path.exists(path):
            os.makedirs(path)
        for i in range(N):
            idate = date - timedelta(i)
            idoy = idate.doy
            fname = '%d%03d.ALM' % (idate.year, idoy)
            if os.path.exists(os.path.join(path, fname)):
                if not local: print 'Almanac file found: ', fname
                break
            fname = '%d%03d.alm' % (idate.year, idoy)
            if os.path.exists(os.path.join(path, fname)):
                if not local: print 'Almanac file found: ', fname
                break
            if not local:
                fname = self.download(idate)
                if fname:
                    break
            fname = False
        if not fname:
            raise RuntimeError('No almanac file found for %s in %s' \
                               % (self.date.date(), path))
        else:
            fid = open_file(os.path.join(path, fname))
            while True:
                try:
                    line = fid.next()
                except StopIteration:
                    break
                if not line: continue
                if line[:2]=='**':
                    ID = 'G%02d' % int(fid.next().split(':')[1])
                    self[ID]={}
                    self[ID]['HEALTH'] = float(fid.next().split(':')[1])
                    self[ID]['ECC']    = float(fid.next().split(':')[1])
                    self[ID]['TOA']    = float(fid.next().split(':')[1])
                    self[ID]['INC']    = float(fid.next().split(':')[1])
                    self[ID]['RRAND']  = float(fid.next().split(':')[1])
                    self[ID]['SMA']    = float(fid.next().split(':')[1])
                    self[ID]['RAND']   = float(fid.next().split(':')[1])
                    self[ID]['AOP']    = float(fid.next().split(':')[1])
                    self[ID]['AM']     = float(fid.next().split(':')[1])
                    self[ID]['AF0']    = float(fid.next().split(':')[1])
                    self[ID]['AF1']    = float(fid.next().split(':')[1])
                    week = int(fid.next().split(':')[1])
                    realweek = idate.GPSweek
                    if week<realweek:
                        week+=1024*(realweek/1024)
                    self[ID]['WK'] = week

    def download(self, date):
        '''
        '''
        toa_1 = ['', '380928', '466944', '552960', '032768', '118784', '']
        toa_2 = ['319488', '405504', '503808', '589824', '061440',
                 '147456', '233472']

        if date<datetime(1996,3,31): toa = toa_1
        else: toa = toa_2

        URL1 = 'http://www.navcen.uscg.gov/?Do=getAlmanac&almanac=%03d&year=%d&format=yuma' \
                % (date.doy, date.year)
        URL2 = 'http://celestrak.com/GPS/almanac/Yuma/%d/almanac.yuma.week%04d.%s.txt' \
                % (date.year, (date.GPSweek)%1024, toa[date.GPSdow])
        URL3 = 'https://gps.afspc.af.mil/gps/archive/%d/almanacs/yuma/wk%d%s.alm' \
                % (date.year, date.GPSweek, date.strftime('%a').lower())
        urls = [URL1, URL2, URL3]

        for url in urls:
            err = True
            try:
                print 'Downloading almanac file from %s' % url
                data = urlopen(url)
                line = data.readline()
                if '****' in line and 'Week' in line:
                    err = False
                    break
            except:
                err = True

        if err: return False
        fname = '%d%03d.ALM' % (date.year, date.doy)
        new_alm = open(os.path.join(self.path, fname), 'w')
        new_alm.write(line+data.read())
        new_alm.close()
        data.close()
        return fname

    def sat_pos(self, prns, dt, user_pos=(0, 0, 0)):
        '''
        Translate from gps.c of "The Essential GNSS Project"
        Copyright (c) 2007 author: doxygen
        '''
        
        # iterate to include the Sagnac correction
        # since the range is unknown, an approximate of 70 ms is good enough
        # to start the iterations so that 2 iterations are enough

        ret   = []
        rango = 0.07*LIGHT_SPEED
        userX, userY, userZ = user_pos
        for prn in prns:
            for i in xrange(2):
                Xs, Ys, Zs = self.calc_sat_pos(prn, dt, rango)
                dx         = Xs - userX
                dy         = Ys - userY
                dz         = Zs - userZ
                rango      = math.sqrt( dx*dx + dy*dy + dz*dz )
            ret.append([Xs, Ys, Zs])

        return ret

    def calc_sat_pos(self, prn, dt, estimate_range):
        '''
        '''
        
        toe = self[prn]['TOA']
        m0  = self[prn]['AM']
        delta_n = 0
        tot = dt.GPSweek*SECONDS_IN_WEEK+dt.GPStow
        tk  = tot - (self[prn]['WK']*SECONDS_IN_WEEK+toe)
        # compute the corrected mean motion
        a   = self[prn]['SMA']**2
        n   = math.sqrt(GPS_GRAVITY_CONSTANT/(a*a*a))
        n  += delta_n
        # kepler's equation for eccentric anomaly
        M = m0 + n*tk
        E = M
        for i in range(8):
            E = M + self[prn]['ECC']*math.sin(E)

        cosE = math.cos(E)
        sinE = math.sin(E)
        # true anomaly
        v = math.atan2(math.sqrt(1.0-self[prn]['ECC']**2)*sinE, (cosE - self[prn]['ECC']))
        #argument of latitude
        u = v + self[prn]['AOP']
        # radius in orbital plane
        r = a * (1.0 - self[prn]['ECC']*math.cos(E))
        # orbital inclination
        i = self[prn]['INC']

        #argument of latitude correction
        #! no correction
        cosu = math.cos(u)
        sinu = math.sin(u)
        x_op = r*cosu
        y_op = r*sinu

        omegak = self[prn]['RAND'] + (self[prn]['RRAND'] - GPS_WGS84_EARTH_ROTATION_RATE)*tk - GPS_WGS84_EARTH_ROTATION_RATE*(toe + estimate_range/LIGHT_SPEED )

        # compute the WGS84 ECEF coordinates,
        # vector r with components x & y is now rotated using, R3(-omegak)*R1(-i)
        cos_omegak = math.cos(omegak)
        sin_omegak = math.sin(omegak)
        cosi = math.cos(i)
        sini = math.sin(i)
        x = x_op * cos_omegak - y_op * sin_omegak * cosi
        y = x_op * sin_omegak + y_op * cos_omegak * cosi
        z = y_op * sini

        return x,y,z

    def get_sat_clock_correction(self, prns, dt):
        '''
        '''
        ret = []
        for prn in prns:
            toe = self[prn]['TOA']
            toc = self[prn]['TOA']
            m0  = self[prn]['AM']
            delta_n = 0
            tot = dt.GPSweek*SECONDS_IN_WEEK+dt.GPStow
            tk  = tot - (self[prn]['WK']*SECONDS_IN_WEEK+toe)
            tc  = tot - (self[prn]['WK']*SECONDS_IN_WEEK + toc)
            # compute the corrected mean motion
            a   = self[prn]['SMA']**2
            n   = math.sqrt(GPS_GRAVITY_CONSTANT/(a*a*a))
            n  += delta_n
            # kepler's equation for eccentric anomaly
            M = m0 + n*tk
            E = M
            for i in xrange(8):
                E = M + self[prn]['ECC']*math.sin(E)

            #relativistic correction
            d_tr  = GPS_CLOCK_CORRECTION_RELATIVISTIC_CONSTANT*self[prn]['ECC']*self[prn]['SMA']*math.sin(E)
            d_tr *= LIGHT_SPEED

            #clock correction
            d_tsv  = self[prn]['AF0']+self[prn]['AF1']*tc#+af2*tc*tc
            ret.append(d_tsv*LIGHT_SPEED+d_tr)

            #clock drift
            #clock_drift = self[prn]['AF1']*LIGHT_SPEED

        return ret

class gpselaz(dict):
    '''
    Class to calculate el, az, lat and lon of satellites, uses yuma almanac info
    '''
    def __init__(self, date, path=None):
        yuma = Yuma(date, path)
        U = 398603.2
        self.date = date
        self.elemnts = {}
        for id in yuma:
            IDate = GPSDateTime(GPSweek=yuma[id]['WK'])
            ragren = self.ra_gren(IDate)
            SMA = yuma[id]['SMA']*yuma[id]['SMA']/1000.0
            #***********************************************************
            #*    CALCULATE PERIOD IN MINUTES FROM SMA                 *
            #*    FORMULA   SMA=(U**1/3)*((86400.0/TWO*PI*N)**2/3)     *
            #*              SMA=SEMI-MAJOR AXIS(KM)                    *
            #*              U=398603.2                                 *
            #*              N=PERIOD(REVS/DAY)                         *
            #***********************************************************
            N = 86400.0/(2*PI)*math.sqrt(U/SMA**3)
            N = 1440.0/N
            #calculate argument of perigee
            AOP = fmod(yuma[id]['AOP'])*rad2deg
            #calculate inclination
            INC = fmod(yuma[id]['INC'])*rad2deg
            RAAN = (yuma[id]['RAND']*rad2deg + ragren)*deg2rad
            RAAN = fmod(RAAN)*rad2deg
            #calculate mean anomaly at 00 hrs
            XMO = 1440.0/N*360.0/86400.0/rad2deg*yuma[id]['TOA']
            AM = fmod(yuma[id]['AM'] - XMO)*rad2deg
            self.elemnts[id] = [IDate, SMA, N, yuma[id]['ECC'], INC, AM, AOP,
                                RAAN]

    def update(self, flat, flon, falt, step=600):
        '''
        Set de location step in seconds
        '''
        #self = {}
        self.step = step
        jyear     = self.date.year
        jmonth    = self.date.month
        jday      = self.date.day
        ndoy      = self.date.doy

        #NAVSTAR PASS PLANNER
        ST2 = [0, 0, 0]
        ST2[0] = (90-flat)*deg2rad
        ST2[1] = flon*deg2rad
        ST2[2] = falt/1000+6370         #earth radius = 6370 km
        #Change IRPT setting according to time step
        IRPT = 86400/step

        if jyear%4 == 0: YRDAYS = 366
        else: YRDAYS = 365

        TRSS  = (ndoy-1)*24.0*60.0
        SILAT = 0
        SILON = 0

        for prn, elem in self.elemnts.items():
            #prn = 'G%02d' % sat
            self[prn] = {}
            self[prn]['hour'] = []
            self[prn]['az'] = []
            self[prn]['el'] = []
            self[prn]['lat'] = []
            self[prn]['lon'] = []
            [TEP,IEY,AO,EO,ONO,OPO,XIO,XLO,XNO,THGR] = self.KBNV(*elem)
            TW  = TRSS - TEP
            TWS = TW*60.0
            x365 = 365
            if IEY%4 == 0: x365 = 366
            for I in range(int(IRPT)):
                RNGP = 0
                TW = TWS/60.0
                TP = TW + TEP
                [JDY,IHR,AMN] = min2date(TP)
                JDYP = JDY
                if (JDY > YRDAYS): JDYP = JDY - YRDAYS
                IMN = int(AMN)
                FMN = AMN - float(IMN)
                SC = FMN*60.0
                TW2W = TW
                if (TW2W < -28800.0):
                    TW2W = TW2W + x365*1440
                [BGR,GCR] = self.SGPN(TW2W,AO,EO,ONO,OPO,XIO,XLO,XNO,THGR)
                [SL2,D] = self.SPGM(ST2,BGR)
                EL2    = 90.0 - SL2[0]*rad2deg
                A1     = EL2
                RNG    = SL2[2]
                if (A1 < -3.0):
                    RNGP = RNG
                    TWS  = TWS + step
                    continue
                ELS    = A1
                SATLAT = GCR[0]
                SATLON = GCR[1]
                SATHT  = GCR[2]
                ELR    = EL2*deg2rad
                AZR    = (SL2[1])
                AZS    = AZR*rad2deg
                if (AZS < 0.0):
                    AZS = AZS + 360.0
                RDOT   = (RNG-RNGP)/step
                XLAM   = 300.0/1575.42
                DOP    = -(RDOT/XLAM)
                if (ELS > 0):
                    [SILAT,SILON,A,RANGE] = SILL(ELS,AZS,350.0,flon,flat)
                self[prn]['hour'].append(IHR+IMN/60.)
                self[prn]['az'].append(AZS)
                self[prn]['el'].append(ELS)
                self[prn]['lat'].append(SILAT)
                self[prn]['lon'].append(SILON)
                RNGP = RNG
                TWS  = TWS + step

    def find(self, prn, itime):
        '''
        Find azimuth, elevation, latitud and longitud, given PRN and time.
        '''
        factor = 3600/self.step
        xaz, xel, xlat, xlon = 0, 0, 0, 0
        eatimes = self[prn]['hour']
        kk = len(eatimes)-1
        dif = [abs(itime-t) for t in eatimes]
        idx = dif.index(min(dif))
        if idx==0:
            i0, i1, i2 = 0, 0, 1
        elif idx==kk:
            i0, i1, i2 = kk, kk-1, kk
        else:
            idif1 = abs(itime - eatimes[idx-1])
            idif2 = abs(itime - eatimes[idx+1])
            i0, i1, i2 = idx - 1, idx -1, idx
            if idif2 < idif1:
                i0, i1, i2 = idx, idx, idx +1
        az1 = self[prn]['az'][i1]
        az2 = self[prn]['az'][i2]
        if (az1-az2) > 330.0:
            if az1 < az2:
                az1 = az1 + 360.0
            elif az1 > az2:
                az1 = az1 - 360.0
        xaz = az1 + (az2 - az1)*(itime-eatimes[i1])*factor
        xel = self[prn]['el'][i0] + (self[prn]['el'][i2] - \
                self[prn]['el'][i1])*(itime-eatimes[i0])*factor
        xlat = self[prn]['lat'][i0] + (self[prn]['lat'][i2] - \
                self[prn]['lat'][i1])*(itime-eatimes[i0])*factor
        xlon = self[prn]['lon'][i0] + (self[prn]['lon'][i2] - \
                self[prn]['lon'][i1])*(itime-eatimes[i0])*factor
        return xaz, xel, xlat, xlon

    def ra_gren(self, idate):
        '''
        Returns the right ascension at Greenwich on idate at 00 hours.
        '''
        dayjul0 = 2433282.5
        sec1 = 8640184
        sec2 = 0.812866
        sec3 = 0.093104
        sec4 = 6.2e-6
        consecs = 24110.54841

        yr = idate.year - 1900
        mn = idate.month
        dy = idate.day

        if mn <= 2:
            mn += 9
            yr -= 1
        else:
            mn -= 3

        #days elapsed from FEB. 29, 1900 TO JAN. 1, 1950 = 18204
        dayjul = ((1461*yr)/4) + (((153*mn)+2)/5) + dy - 18204 + dayjul0
        T = (dayjul - 2451545.)/36525.
        AA1 = sec1*T + T*(sec2 + T*(sec3 - sec4*T))
        ragren = consecs/240. + AA1/240.
        ragren = ragren%360
        if ragren < 0:
            ragren = ragren + 360
        return ragren

    def KBNV(self, idate, AO, PERM, EO, XI, G, OPO, ONO):
        '''
        Disc file read of GPS Keplerian elements
        elem = [IDate,SMA,N,ECC,INC,AM,AOP,RAAN]
        '''

        THGS = 99.24377
        SIDR = 0.985647346
        SDAY = 7304
        IED = idate.doy
        IEY = idate.year-1900
        if EO == 0:
            return [None for x in range(12)]
        else:
            IEPT = IED - 1 + (IEY - 50)*365 + (IEY - 49)/4
            FEPT = float(IEPT)
            THGR = ((FEPT - SDAY)*SIDR + THGS)*deg2rad
            THGR = fmod(THGR)
            TE   = (IED - 1)*1440.0
            XNO  = 2*PI/PERM
            ONO  = ONO*deg2rad
            XIO  = XI*deg2rad
            OPO  = OPO*deg2rad
            G    = G*deg2rad
            XLO  = fmod(ONO + OPO + G)
            AO   = AO/6378.166
            return TE,IEY,AO,EO,ONO,OPO,XIO,XLO,XNO,THGR

    def SGPN(self, T,AO,EO,ONO,OPO,XIO,XLO,XNO,THGR):
        '''
        THIS IS A VERSION OF THE NORAD
        SIMPLIFIED GENERAL PERTURBATIONS (SGP)
        CODE FOR THE PROPAGATION OF NORAD MEAN
        ORBITAL ELEMENTS TO TIME T...
        ...INITIALIZATION BY SUBROUTINE KBNO
        VERSION FOR NAG ELEMENTS...
        '''

        GCN=[0,0,0]
        XJ2 =0.0010826158
        SIDD = 0.4375269512707e-2
        XL=XLO+T*XNO
        XL=fmod(XL)
        ON=fmod(ONO)
        OP=fmod(OPO)
        E=EO
        SOP=math.sin(OP)
        COP=math.cos(OP)
        AXN=E*COP
        AYN=E*SOP
        SI=math.sin(XIO)
        CI=(XIO)
        ON=fmod(ON)
        OP=fmod(OP)
        XL=fmod(XL)
        U=fmod(XL-ON)
        TP1=XJ2*(1.0-1.5*SI*SI)/(2.0*AO*AO*(1.0-E*E))
        A=AO*(1.0+TP1)
        IT=50
        TP1=U
        TP2=1.0
        E6A=1.0e-8*TP1+1.0e-6
        while (abs(TP2)>=E6A):
            SINEO=math.sin(TP1)
            COSEO=math.cos(TP1)
            IT=IT-1
            if (IT<=0):
                err=True
            TP5=1.0-COSEO*AXN-SINEO*AYN
            TP5=(U-(-SINEO*AXN+COSEO*AYN+TP1))/TP5
            if (1.0<abs(TP5)):
                TP5=DABS(TP5)/TP5
            TP2=TP5
            TP1=TP2+TP1
        ESE=-AYN*COSEO+AXN*SINEO
        ECE=AXN*COSEO+AYN*SINEO
        ESQ=AXN*AXN+AYN*AYN
        E=math.sqrt(ESQ)
        R=A*(1.0-ECE)
        P=A*(1.0-ESQ)
        TP1=1.0+math.sqrt(1.0-ESQ)
        SSU=ESE*AXN/TP1
        CSU=(ESE/TP1*AYN-AXN+COSEO)/R*A
        SSU=A*(-SSU-AYN+SINEO)/R
        SU=self.ACTN(SSU,CSU)
        C2SU=2.0*SU
        S2SU=math.sin(C2SU)
        C2SU=math.cos(C2SU)
        TP7=XIO
        SI=math.sin(TP7)
        CI=math.cos(TP7)
        S2I=math.sin(2.0*TP7)
        FA=3.0*XJ2/(2.0*P*P)
        DR=math.atan(SSU/CSU)
        RD=math.atan(SSU/CSU)
        RD=abs(math.cos(RD))
        DLR=RD*FA/3.0*SI*SI*P
        DLSU=-(-7.0*SI*SI+6.0)/12.0*S2SU*FA
        DLON=FA*S2SU/2.0*CI
        DLXI=S2I*C2SU/4.0*FA
        R=R+DLR
        SU=fmod(SU+DLSU)
        ON=fmod(ON+DLON)
        SOP=AYN/E
        COP=AXN/E
        OP=self.ACTN(SOP,COP)
        XI=XIO+DLXI
        SON=math.sin(ON)
        CON=math.cos(ON)
        SI=math.sin(XI)
        CI=math.cos(XI)
        SSU=math.sin(SU)
        CSU=math.cos(SU)
        X=CSU
        Y=CI*SSU
        Z=SI*SSU
        HRA=THGR+T*SIDD
        HRA=fmod(HRA)
        GCN[1]=self.ACTN(Y,X)+ON-HRA
        GCN[1]=fmod(GCN[1])
        XY=math.sqrt(X*X+Y*Y)
        GCN[0]=self.ACTN(XY,Z)
        GCN[2]=R*6378.166
        [GCD,GCR] = TRNN(GCN[0], GCN[1], GCN[2])
        return GCD,GCR

    def SPGM(self, RO, RT):

        RB=[0,0,0]
        CTO = math.cos(RO[0])
        STO = math.sin(RO[0])
        CTT = math.cos(RT[0])
        STT = math.sin(RT[0])
        CHD = CTO*CTT+STO*STT*math.cos(RT[1]-RO[1])
        SHD = math.sqrt(1.0-CHD*CHD)
        GAM = math.atan2(SHD,CHD)
        SBE = STT*math.sin(RT[1]-RO[1])/SHD
        CBE = (CTT-CTO*CHD)/(STO*SHD)
        RB[1] = math.atan2(SBE,CBE)
        RB[2] = math.sqrt(RO[2]*RO[2]+RT[2]*RT[2]-2.0*RO[2]*RT[2]*CHD)
        SEL   = RT[2]*SHD/RB[2]
        CEL   = (RT[2]*CHD-RO[2])/RB[2]
        RB[0] = math.atan2(SEL,CEL)
        #print 'spgm ',RO,RT
        return RB,GAM

    def ACTN(self, SX, CX):
        ACTN = 0.0
        if (CX == 0.0):
            if (SX == 0.0):
                return 0.0
            elif (SX > 0.0):
                return PI/2
            else:
                return 3*PI/2
        elif (CX > 0.0):
            if (SX == 0.0):
                return 0.0
            elif (SX > 0.0):
                return math.atan(SX/CX)
            else:
                return 2.0*PI + math.atan(SX/CX)
        else:
            return PI + math.atan(SX/CX)

if __name__=='__main__':
    yuma = Yuma(GPSDateTime(1996,6,29))
    print xyz2lla(1406890.1145,-6077658.6177,-1325066.7848)
